# coding: utf-8
import roslibpy
import time
import roslibpy.actionlib
import math
import logging
import sys

ROS_MASTER = '192.168.100.21'


class PeanutROS:
    def __init__(self, master_url=ROS_MASTER):
        # logger
        LOG_FORMAT = "%(asctime)s[ROS]%(message)s"
        DATE_FORMAT = "[%Y/%m/%d %H:%M:%S]"

        self.logger = logging.getLogger('ros')
        self.logger.setLevel(logging.INFO)
        formatter = logging.Formatter(fmt=LOG_FORMAT, datefmt=DATE_FORMAT)
        handler = logging.StreamHandler(sys.stdout)
        handler.setFormatter(formatter)
        self.logger.addHandler(handler)

        # 连接到 ros master
        self.client = roslibpy.Ros(host=master_url, port=9090)
        while not self.client.is_connected:
            self.client.run(timeout=10)
            if self.client.is_connected:
                break
            else:
                self.client.close()
                self.logger.error("初始化过程中无法连接到 ROS, 重试中...")
                time.sleep(5)

        while not self.client.is_connected:
            self.logger.info(
                'ROS master {} 无法连接, 重试中...'.format(master_url)
            )
            time.sleep(3)
            self.client.run()
        if not self.client.is_connected:
            self.logger.error('无法连接到 ROS master {}'.format(master_url))
        else:
            self.logger.info('连接到 ROS master {}'.format(master_url))

        # Publishers
        # 充电话题
        self.charge_topic = roslibpy.Topic(
            self.client, '/app_cmd_charge', 'keenon_ui_msgs/pose_config'
        )
        self.charge_topic.advertise()

        # 电机话题
        self.motor_topic = roslibpy.Topic(self.client, '/motor_lock', 'std_msgs/Bool')
        self.motor_topic.advertise()

        # 运动控制话题
        self.cmd_topic = roslibpy.Topic(
            self.client, '/cmd_vel', 'geometry_msgs/Twist', throttle_rate=50
        )
        self.cmd_topic.advertise()

        # 导航服务
        self.target_service = roslibpy.Service(
            self.client, '/web_pose_config', 'keenon_ui_msgs/web_pose_config'
        )

        # 导航目标话题
        self.target_topic = roslibpy.Topic(
            self.client, '/move_base_simple/goal', 'geometry_msgs/PoseStamped'
        )
        self.target_topic.advertise()

        # 停止移动服务
        self.stop_robot_service = roslibpy.Service(
            self.client, '/ui_cmd/command_server', 'keenon_ui_msgs/ctrlcommand'
        )

        # Subscribers
        # 导航结果话题
        self.result_topic = roslibpy.Topic(
            self.client,
            '/move_base/result',
            'keenon_move_base_msgs/MoveBaseActionResult',
        )
        self.goal_reached = False  # 是否到达导航目标
        self.result_topic.subscribe(self.move_base_res_callback)

        # 称重话题
        self.weighing_topic = roslibpy.Topic(
            self.client, '/weighing_scale/data', 'std_msgs/String'
        )
        self.weighing_topic.subscribe(self.weighing_callback)
        self.cur_weighing = 0.0

        # unused
        # 目标点，与网页后台同步
        # self.target_points = self.get_target_points()
        # self.client.set_param('/move_base/local_costmap/inflation_layer/inflation_radius', 0)

    def move_base_res_callback(self, event):
        """move_base 结果的回调函数

        Args:
            event (event): 回调事件
        """
        if event['status']['text'] == 'Goal reached' or event['status']['status'] == 3:
            self.logger.info('到达目标位置')
            self.goal_reached = True

    def weighing_callback(self, event):
        """称重话题的回调函数

        Args:
            event (event): 回调事件
        """
        # 这里得到的值单位是 0.1g
        self.cur_weighing = -float(event['data'])   # 取负是因为称重模块装反了……


    def unlock_motor(self):
        """解锁机器人电机, 与网页后台对应按钮效果相同.
        """
        self.motor_topic.publish(roslibpy.Message({'data': False}))
        self.logger.info('电机已解锁')

    def stop_charge(self):
        """通过 ROS 发布停止充电指令. 一般情况下只需要使用 app 的 stop_charge 方法即可.
        """
        msg = {
            'floor': 0,
            'type': 'charge_stop',
            'id': 0,
            'pose': {
                'position': {'y': 0.0, 'x': 0.0, 'z': 0.0},
                'orientation': {'y': 0.0, 'x': 0.0, 'z': 0.0, 'w': 0.0},
            },
            'name': 'stop charge cmd',
        }
        self.charge_topic.publish(roslibpy.Message(msg))
        self.logger.info('停止充电') # 调用太频繁了, 先 disable

    def charge(self):
        """通过 ROS 发布充电指令, 一般情况下只需要使用 app 的 charge 方法即可.
        """
        msg = {
            'floor': 0,
            'type': 'charge_auto',
            'id': 0,
            'pose': {
                'position': {'y': 0.0, 'x': 0.0, 'z': 0.0},
                'orientation': {'y': 0.0, 'x': 0.0, 'z': 0.0, 'w': 0.0},
            },
            'name': 'auto charge cmd',
        }
        self.charge_topic.publish(roslibpy.Message(msg))
        self.logger.info('返回充电')

    def get_target_points(self, type='normal', floor=1):
        """获取目标点

        Args:
            type (str, optional): 目标点类型. Defaults to 'normal'.
            floor (int, optional): 目标点所在楼层. Defaults to 1.

        Returns:
            list: 包含目标点的数组, 目标点的数据结构为: 
            ```
            {
                floor   (int): 目标点所在楼层.
                type    (str): 目标点类型.
                id      (str): 目标点 id.
                name    (str): 目标点名称
                pose: {
                    position: {x, y, z}
                    orientation: {x, y, z, w}
                }
            }
            ```
        """
        req = {
            'opt': 'select',
            'request': [
                {
                    'id': 1,
                    'type': type,
                    'name': '',
                    'floor': floor,
                    'pose': {
                        'position': {'x': -1, 'y': -1, 'z': -1},
                        'orientation': {'x': -1, 'y': -1, 'z': -1, 'w': -1},
                    },
                }
            ],
        }
        target_request = roslibpy.ServiceRequest(req)
        response = self.target_service.call(target_request)
        if response['msg'] == 'SUCCESS':
            self.logger.info('目标点列表更新成功')
            return response['response']
        else:
            self.logger.warning('无法更新目标点列表')
            return []

    def delete_target_point(self, target):
        """删除给定的目标点，并返回该目标点

        Args:
            target (dict): 与 get_target_points 中获得的目标点数据结构相同

        Returns:
            dict: 被删除的目标点
        """
        req = {'opt': 'delete', 'request': [target]}
        target_request = roslibpy.ServiceRequest(req)
        response = self.target_service.call(target_request)
        if response['msg'] == 'SUCCESS':
            self.logger.info(
                '删除目标点: id: {0}, name: {1}'.format(
                    target['id'], target['name']
                )
            )
            return response['response']
        else:
            self.logger.warning(
                '无法删除目标点: id: {0}, name: {1}'.format(
                    target['id'], target['name']
                )
            )
            return []

    def insert_target_point(self, target):
        """插入给定的目标点，并返回该目标点

        Args:
            target (dict): 与 get_target_points 中获得的目标点数据结构相同

        Returns:
            dict: 插入的目标点
        """
        req = {'opt': 'insert', 'request': [target]}
        target_request = roslibpy.ServiceRequest(req)
        response = self.target_service.call(target_request)
        if response['msg'] == 'SUCCESS':
            self.logger.info(
                '新增目标点: id: {0}, name: {1}'.format(
                    target['id'], target['name']
                )
            )
            return response['response']
        else:
            self.logger.warning(
                '无法新增目标点: id: {0}, name: {1}'.format(
                    target['id'], target['name']
                )
            )
            return []

    def move_by(self, dis=0.0, angle=0.0, linear_speed=0.0, angular_speed=0.0):
        """控制机器人执行平移和旋转操作

        Args:
            dis (float, optional): x 轴方向的位移, 单位为米, 负数为向后移动. Defaults to 0.0.
            angle (float, optional): z 轴的旋转, 单位为度. 正数为逆时针, 负数为顺时针. Defaults to 0.0.
            linear_speed (float, optional): x 轴位移线速度, 不允许为负数. Defaults to 0.0.
            angular_speed (float, optional): z 轴位移角速度，不允许为负数. Defaults to 0.0.
        """
        if linear_speed < 0:
            self.logger.error('线速度不能小于 0')
            return
        if angular_speed < 0:
            self.logger.error('角速度不能小于 0')
            return
        msg = roslibpy.Message(
            {
                'linear': {'x': 0.0, 'y': 0.0, 'z': 0.0},
                'angular': {'x': 0.0, 'y': 0.0, 'z': 0.0},
            }
        )
        backward = False
        # 平移旋转为负值的处理
        if dis < 0:
            backward = True
            dis = -dis
        clockwise = False
        if angle < 0:
            clockwise = True
            angle = -angle
        # 运动时间计算
        linear_duration = 0.0
        angular_duration = 0.0
        if linear_speed != 0.0:
            linear_duration = dis / linear_speed
        if angular_speed != 0.0:
            angle = math.radians(angle)
            angular_duration = angle / angular_speed

        msg['linear']['x'] = linear_speed if not backward else -linear_speed    # move forward
        ticks = int(linear_duration / (self.cmd_topic.throttle_rate / 1000))    # 时钟
        for _ in range(ticks):
            self.cmd_topic.publish(msg)
            time.sleep(self.cmd_topic.throttle_rate / 1000)
        msg['linear']['x'] = 0.0
        self.cmd_topic.publish(msg)
        time.sleep(1)

        msg['angular']['z'] = angular_speed if not clockwise else -angular_speed    # rotate
        ticks = int(angular_duration / (self.cmd_topic.throttle_rate / 1000))
        for _ in range(ticks):
            self.cmd_topic.publish(msg)
            time.sleep(self.cmd_topic.throttle_rate / 1000)
        msg['angular']['z'] = 0.0
        self.cmd_topic.publish(msg)
        time.sleep(1)

    def move_to(self, pose):
        """移动机器人到对应位置

        Args:
            pose (geometry_msgs/Pose): Target pose for the robot to move to.
        """
        self.goal_reached = False
        msg = roslibpy.Message(
            {
                'header': {'stamp': {'secs': 0, 'nsecs': 100}, 'frame_id': 'map'},
                'pose': pose,
            }
        )
        self.target_topic.publish(msg)

    def stop_move(self):
        """停止机器人移动
        """
        res = self.stop_robot_service.call(roslibpy.ServiceRequest({'command': 1}))
        self.logger.info('停止移动')
        self.goal_reached = False
